package robot.calibeta;

import java.util.ArrayList;
import java.util.Hashtable;

import lejos.nxt.ColorLightSensor;
import lejos.nxt.SensorPort;
import lejos.robotics.Colors.Color;
import chair.Brain;
import chair.Organ;
import chair.core.Diary;

/**
 * Les cellules en bâtonnet du robot : utilise le capteur de lumière afin de
 * déterminer quelle est la couleur située sous lui. Attend que le robot soit à
 * l'arrêt ou se soit déplacé un minimum avant de décider qu'on rencontre
 * effectivement une nouvelle couleur. Dans ce dernier cas il lui faut de plus
 * rencontrer deux fois la couleur consécutivement avant de la valider, ceci
 * afin d'éviter de tromper le capteur quand il est à cheval entre du noir et du
 * blanc.
 * 
 * Enfin, avec l'utilisation de ColorLightSensorFix, ne va effectuer de mesures
 * que si Motion n'est pas occupé pour économiser la bande passante en cas
 * d'utilisation distance.
 * 
 * Cette classe s'occupe aussi de détecter des pièces, représentées sous la
 * forme de deux carrés de couleur. Si deux couleurs consécutives sont
 * rencontrées (autre que en premier rouge : carrefour et noir : piste ; le
 * blanc compte pour du beurre vu le déplacement en zigzags du robot). Possible
 * de coder 9 salles en tout.
 * 
 * FIXME: actuellement le rouge ne compte pas comme seconde couleur de salle,
 * risque d'erreur lorsque mauvaise lecture de ligne noir avant de rencontrer
 * carrefour.
 * 
 * Va enregistrer de nouvelles sensations à la volée quand rencontrera de
 * nouvelles salles.
 * 
 * @author nomhad
 * 
 */
public class OrganEyeColor extends Organ {
	/**
	 * Il faut que le robot ait parcourue XXmm pour que l'on considère que la
	 * nouvelle couleur comme potentiellement valable
	 */
	private static final float SIGNIFICANT_DISTANCE = 3;

	/**
	 * Combien de mm peut parcourir le robot au minimum avant de rencontrer
	 * nouveau carrefour. Utilisé afin de ne pas risquer de prendre le même
	 * carré rouge deux fois de suite pour un nouveau carrefour
	 */
	private static final float SIGNIFICANT_DISTANCE_BETWEEN_JUNCTION = 70;

	/**
	 * On ne va pas lever deux fois sensation carrefour si pas eu le temps de
	 * s'en éloigné
	 */
	private boolean _hasTravelledFromJunction = true;
	/**
	 * Si on est arrivé sur une salle, on ne veut pas la signaler à nouveau le
	 * temps de la quitter
	 */
	private boolean _hasTravelledFromRoom = true;
	/**
	 * Distance totale parcourue lors de la rencontre avec le dernier carrefour
	 * (utilisé pour savoir si possible d'en rencontrer un nouveau). Initialisé
	 * de telle sorte que premier carrefour sera considéré, avec distance
	 * possible de 10cm)
	 */
	private float _totalTravelledDistanceSinceJunction = -(SIGNIFICANT_DISTANCE_BETWEEN_JUNCTION + 100);
	/**
	 * Sauvegarde de la distance parcourue depuis dernier carrefour pendant que
	 * passif
	 */
	private float _backupTravelledDiffSinceJunction;
	/** Le capteur de lumière */
	private ColorLightSensor _cls;
	/** Couleur actuellement perçue par l'organe */
	private Color _currentColor;
	/**
	 * La dernière couleur mesurée. Il faut avoir vu deux fois la même couleur
	 * consécutivement pour qu'elle soit désignée comme couleur actuelle
	 */
	private Color _lastColor;
	/** Sauvegarde _currentColor lorsqu'en mode passif */
	private Color _backupCurrentColor;
	/** Sauvegarde _lastColor lorsqu'en mode passif */
	private Color _backupLastColor;
	/** Distance parcourue par le robot lors de la dernière interrogation */
	private float _lastTravelledDistance;
	/**
	 * On a croisé un peu de couleur, on attend un second patch pour savoir si
	 * on a détecté une salle
	 */
	private Color _roomCandidate;
	/**
	 * La pièce sur laquelle se trouve le robot, codé sous la forme
	 * "couleur-couleur"
	 */
	private String _currentRoom;
	/** Évite de devoir récupérer valeur de la salle tant qu'on est dessus */
	private byte _currentRoomValue = -1;
	/** Quelles sont les salles déjà rencontrées ? */
	private ArrayList<String> _visitedRooms;
	/** Quelles sont les valeurs de senseurs associées ? */
	private Hashtable _visitedRoomsValues;

	/**
	 * Cet organe a la particularité d'enregistrer de nouvelles sensations à la
	 * volée (découverte d'une nouvelle salle). Il doit pour cela savoir à
	 * partir de quelle valeur il peut les renseigner.
	 */
	private byte freeSense;

	public OrganEyeColor(Brain brain) {
		super(brain, "OrganEyeColor");
		_cls = new ColorLightSensorFix(SensorPort.S3,
				ColorLightSensor.TYPE_COLORFULL);
		_visitedRooms = new ArrayList<String>();
		_visitedRoomsValues = new Hashtable();
		// 1: blanc, 2: rouge (marqueur de changement de direction)
		byte[] values = { 1, 2 };
		// pièces commenceront à partir de 3
		freeSense = 3;
		String[] names = { "HorsPiste", "Carrefour" };
		registerValues(values, names);
		// On veut savoir au plus vite sur quoi on se trouve
		_currentColor = _cls.readColor();
	}

	/**
	 * On a rencontré une pièce : cette méthode s'occupe d'enregistrer nouvelle
	 * sensation si c'est une découverte et de mettre à jour _currentRoomeValue
	 */
	private void onRoom() {
		// Valeur associée à la salle
		_currentRoomValue = -1;
		synchronized (_visitedRooms) {
			synchronized (_visitedRoomsValues) {
				if (!_visitedRooms.contains(_currentRoom)) {
					Diary.logln("Découverte d'une pièce ! ");
					_currentRoomValue = freeSense++;
					_visitedRooms.add(_currentRoom);
					_visitedRoomsValues.put(_currentRoom, new Byte(
							_currentRoomValue));
				}
			}
		}
		// On a initialisé valeur, c'était une pièce jamais rencontrée
		if (_currentRoomValue != -1)
			newRoom();
		// Sinon on pioche
		else
			synchronized (_visitedRoomsValues) {
				_currentRoomValue = ((Byte) _visitedRoomsValues
						.get(_currentRoom)).byteValue();
			}
	}

	/**
	 * onRoom a détecté qu'on venait de découvrir une nouvelle pièce : on
	 * enregistre nouvelle sensation.
	 */
	private void newRoom() {
		byte[] values = { _currentRoomValue };
		String[] names = { _currentRoom };
		registerValues(values, names);
	}

	@Override
	protected byte sense() {
		// distance parcourue par le robot
		float distance = Motion.getTravelDistance();
		// Si le robot est à l'arrêt on regarde couleur et puis c'est tout.
		// Typiquement : Cartographer explore carrefour et attend réponse.
		if (Motion.isOnABreak()) {
			_currentColor = _cls.readColor();
		}
		// Sinon on prend quelques précautions avant de répondre
		else {
			// On s'est suffisamment déplacé : la couleur actuelle est un
			// candidat valable
			if (Math.abs(distance - _lastTravelledDistance) > SIGNIFICANT_DISTANCE) {
				// RAZ avec nouveau candidat
				_lastTravelledDistance = distance;
				Color newColor = _cls.readColor();
				// Si c'est la deuxième fois qu'on mesure la couleur alors
				// elle est digne de confiance. Sinon on attendra nouveau
				// candidat.
				if (newColor == _lastColor)
					_currentColor = newColor;
				else
					_lastColor = newColor;
			}
		}
		switch (_currentColor) {
		case NONE:
			// Erreur de casting, on passe
			return 0;
		case WHITE:
			// Le néant marque un hors-piste
			return 1;
		case BLACK:
			// Le noir remet à 0 le compteur de salle ainsi que la salle en
			// cours. Ne signifie rien en lui-même. Un simple vortex qui
			// concentre toutes vos peurs. NB : vaut mieux qu'il sépare
			// carrefours et salles
			_roomCandidate = null;
			_currentRoom = null;
			_hasTravelledFromRoom = true;
			// autre RAZ : peut s'attendre à nouveau carrefour
			// NB: si passif Cartographer sonde autour carrefour
			if (!_hasTravelledFromJunction && !isPassive()) {
				_hasTravelledFromJunction = true;
				_totalTravelledDistanceSinceJunction = distance;
				// Diary.logln("Peut rencontrer à nouveau carrefour, distance : "
				// + distance);
			}
			return 0;
		case RED:
			// FIXME: empêche rouge d'être une couleur de salle
			_roomCandidate = null;
			// Le rouge en première couleur indique un carrefour
			if (_roomCandidate == null) {
				// Contrôle qu'on s'est suffisamment éloigné de précédent
				// carrefour
				if (_hasTravelledFromJunction
						&& Math.abs(distance
								- _totalTravelledDistanceSinceJunction) >= SIGNIFICANT_DISTANCE_BETWEEN_JUNCTION) {
					_totalTravelledDistanceSinceJunction = distance;
					_hasTravelledFromJunction = false;
					return 2;
				}
				// sinon valeur par défaut
				else
					return 0;
			}
		}
		// Si en mode passif on ne veut pas tester les salles, renvoie valeur
		// par défaut
		if (isPassive())
			return 0;
		// Si on est pas déjà sur un pièce et que pas à découvrir piste pour
		// Cartographer
		if (_currentRoom == null) {
			// Si la première couleur qu'on rencontre est nouvelle :
			// candidat à une pièce mais pour le moment non significatif
			if (_roomCandidate == null) {
				_roomCandidate = _currentColor;
				Diary.logln("Une couleur pour salle : " + _roomCandidate);
				return 0;
			} else {
				// Seconde couleur de notre pièce
				if (_roomCandidate != _currentColor) {
					_currentRoom = _roomCandidate.toString() + "-"
							+ _currentColor.toString();
					Diary.logln("Une salle découverte : " + _currentRoom);
					// Laisse la main pour mettre à jour valeur
					onRoom();
					return _currentRoomValue;
				}
				// Sinon toujours la même couleur : encore rien de significatif
				else
					return 0;
			}
		}
		// Ne reste plus que le cas où on est toujours sur un marqueur de
		// salle
		if (_hasTravelledFromRoom) {
			_hasTravelledFromRoom = false;
			// En profite aussi pour signaler qu'on peut pas avoir tout de suite
			// de carrefour (permettra du même coup de mettre à jour
			// _totalTravelledDistanceSinceJunction, utile si vient de se mettre
			// en marche arrière)
			_hasTravelledFromJunction = false;
			return _currentRoomValue;
		} else
			return 0;
	}

	/**
	 * Ajoute au comportement par défaut la sauvegarde du traitement de la
	 * couleur. Permet de revenir à l'état précédent la mise en passivité.
	 * Attention, suppose que le capteur revient à sa position initiale.
	 */
	@Override
	public void setPassive(boolean toggle) {
		// Si on s'apprête à basculer en passif, sauvegarde distance depuis
		// dernier carrefour, ainsi que couleurs
		if (toggle && !isPassive()) {
			_backupCurrentColor = _currentColor;
			_backupLastColor = _lastColor;
			float distance = Motion.getTravelDistance();
			_backupTravelledDiffSinceJunction = distance
					- _totalTravelledDistanceSinceJunction;
		}
		// Passe main à code par défaut
		super.setPassive(toggle);
		// Si on vient de redevenir actif, restaure distance depuis dernier
		// carrefour, ainsi que couleurs
		if (!toggle && !isPassive()) {
			_currentColor = _backupCurrentColor;
			_lastColor = _backupLastColor;
			float distance = Motion.getTravelDistance();
			_totalTravelledDistanceSinceJunction = distance
					- _backupTravelledDiffSinceJunction;
		}
	}
}
